#include "primitive.hpp"
#include "limits"

Primitive::~Primitive()
{
}
Sphere::Sphere()
{
	m_sphere = new NonhierSphere(Point3D(0,0,0),1);
}
Sphere::~Sphere()
{
	delete m_sphere;
}

Cube::Cube()
{
	m_box = new NonhierBox(Point3D(0,0,0),1);
}

Cube::~Cube()
{
	delete m_box;
}

NonhierSphere::~NonhierSphere()
{
}
NonhierBox::~NonhierBox()
{
}

Cylinder::Cylinder(double radius, double height)
{
	m_radius = radius;
	m_height = height;
}
Cylinder::~Cylinder()
{
}

Cone::Cone(double radius, double height)
{
	m_radius = radius;
	m_height = height;
}
Cone::~Cone()
{
}

Torus::Torus(double in_radius, double out_radius)
{
	min_radius = in_radius;
	max_radius = out_radius;
}
Torus::~Torus()
{
}

std::pair<Point3D,Point3D> Sphere::get_voxels()
{
	return m_sphere->get_voxels();
}

std::pair<Point3D,Point3D> Cube::get_voxels()
{
	return m_box->get_voxels();
}

std::pair<Point3D,Point3D> NonhierSphere::get_voxels()
{
	Point3D min_vectex(m_pos[0]-m_radius,m_pos[1]-m_radius,m_pos[2]-m_radius);
	Point3D max_vectex(m_pos[0]+m_radius,m_pos[1]+m_radius,m_pos[2]+m_radius);
	return std::pair<Point3D,Point3D>(min_vectex, max_vectex);					  
}

std::pair<Point3D,Point3D> NonhierBox::get_voxels()
{
	return std::pair<Point3D,Point3D>(near_vectex, far_vectex);
}

std::pair<Point3D,Point3D> Cylinder::get_voxels()
{
	Point3D min_vectex(-m_radius,0.0,-m_radius);
	Point3D max_vectex(m_radius,m_height,m_radius);
	return std::pair<Point3D,Point3D>(min_vectex, max_vectex);
}

std::pair<Point3D,Point3D> Cone::get_voxels()
{
	Point3D min_vectex(-m_radius,0.0, +m_radius);
	Point3D max_vectex(m_radius,m_height,-m_radius);
	return std::pair<Point3D,Point3D>(min_vectex, max_vectex);
}

std::pair<Point3D,Point3D> Torus::get_voxels()
{
	Point3D min_vectex(-max_radius,-max_radius-min_radius,-max_radius);
	Point3D max_vectex(max_radius,max_radius+min_radius,max_radius);
	return std::pair<Point3D,Point3D>(min_vectex, max_vectex);
}

bool Torus::RayIntersect(Ray ray, Intersection *intersection)
{
	Point3D pos = ray.get_position();
	Vector3D dir = ray.get_direction();
	Vector3D orgTopos = pos - Point3D(0,0,0);
	double sqr1 = pow(min_radius,2);
	double sqr2 = pow(max_radius,2);
	double a = dir.dot(dir);
	double b = 2 * (orgTopos.dot(dir));
	double c = (orgTopos.dot(orgTopos)) - sqr1 - sqr2; 
	double A = pow(a,2);
	double B = 2*a*b;
	double C = pow(b,2) + 2*a*c + 4*sqr2*pow(dir[2],2);
	double D = 2*b*c + 8*sqr2*pos[2]*dir[2];
	double E = pow(c,2) + 4*sqr2*pow(pos[2],2) - 4*sqr1*sqr2;
	bool hit = false;
	double roots[4];
	int num_root = quarticRoots(B/A, C/A, D/A, E/A, roots);
	double u_max = INT_MIN;
    Vector3D n_max;
	for(int i = 0; i < num_root; i++){
		double u = roots[i];
		if(u > 1e-6 && u_max < u){
            Point3D point = ray.get_position() + u*(ray.get_direction());
                u_max = u;
				double sqx = pow(point[0],2);
				double sqy = pow(point[1],2);
				double X = point[0] - point[0]*max_radius/sqrt(sqx+sqy);
				double Y = point[1] - point[1]*max_radius/sqrt(sqx+sqy);
                n_max = Vector3D(X,Y,point[2]);
                n_max.normalize();
        }
		if(u > 1e-6 && u < intersection->m_dist){
			Point3D point = ray.get_position() + u*(ray.get_direction());
			intersection->m_dist = u;
			intersection->m_intersect = point;
			double sqx = pow(point[0],2);
			double sqy = pow(point[1],2);
			double X = point[0] - point[0]*max_radius/sqrt(sqx+sqy);
			double Y = point[1] - point[1]*max_radius/sqrt(sqx+sqy);
			intersection->m_normal = Vector3D(X,Y,point[2]);
			intersection->u = (1.0 - (atan2(point[1], point[0]) + M_PI) / (M_PI*2));
			double len = sqrt(sqx+sqy);
			intersection->v = (atan2(point[2],(len-max_radius))+M_PI)/(2*M_PI);
			hit = true;
		}
		if(hit){
	        intersection->m_dist2 = u_max;
	        intersection->m_normal2 = n_max;
	    }
	}
	return hit;
}

// X^2 + Z^2 = r^2/h^2 * (Y^2 - h)^2
bool Cone::RayIntersect(Ray ray, Intersection *intersection)
{
	Vector3D ray_dir = ray.get_direction();
	Point3D ray_pos = ray.get_position();
	double RH = pow(m_radius,2)/pow(m_height,2);
	double A = pow(ray_dir[0],2)+pow(ray_dir[2],2)-RH*pow(ray_dir[1],2);
	double B = 2*(ray_pos[0]*ray_dir[0]+ray_pos[2]*ray_dir[2]-RH*ray_dir[1]*ray_pos[1]);
	double C = pow(ray_pos[0],2)+pow(ray_pos[2],2)-RH*pow(ray_pos[1],2);
	double roots[2];
	int num_root = quadraticRoots(A,B,C,roots);
	//if(num_root == 0) return false;
    double u_max = INT_MIN;
    Vector3D n_max;
	bool hit = false;
	for(int i=0; i<num_root; i++){
		double u = roots[i];
        if(u > 1e-6 && u_max < u){
            Point3D temp_point = ray.get_position() + u*(ray.get_direction());
            if(temp_point[1] >=0 && temp_point[1] <= m_height){
                u_max = u;
                n_max = Vector3D(temp_point[0],-RH*temp_point[1],temp_point[2]);
                n_max.normalize();
            }
        }
		if(u > 1e-6 && u < intersection->m_dist){
			Point3D temp_point = ray.get_position() + u*(ray.get_direction());
			if( temp_point[1] >= 0 && temp_point[1] <= m_height){
				intersection->m_dist = u;
				intersection->m_intersect = temp_point;
				intersection->m_normal = Vector3D(temp_point[0],-RH*temp_point[1],temp_point[2]);
				intersection->m_normal.normalize();
				double phi = atan2(intersection->m_normal[2], intersection->m_normal[0]);
				if(phi < 0)
					phi = -phi;
				else
					phi = M_PI - phi;
				intersection->u = phi/(M_PI);
				intersection->v = 1.0-temp_point[1]/m_height;
				hit = true;
			}
		}
	}
	if(fabsf(ray_dir[1]) > 1e-6){
		double t = (m_height - ray_pos[1])/ ray_dir[1];
        if(t > 1e-6 && u_max < t){
            Point3D temp_point = ray.get_position() + t*(ray.get_direction());
			if(pow(temp_point[0],2) + pow(temp_point[2],2) <= pow(m_radius,2)){
				u_max = t;
				n_max = Vector3D(0,1,0);
			}
        }
		if(t > 1e-6 && t < intersection->m_dist){
			Point3D temp_point = ray.get_position() + t*(ray.get_direction());
			if(pow(temp_point[0],2) + pow(temp_point[2],2) <= pow(m_radius,2)){
				intersection->m_dist = t;
				intersection->m_intersect = temp_point;
				intersection->m_normal = Vector3D(0,1,0);
				intersection->u = (temp_point[0]+m_radius)/(2*m_radius);
				intersection->v = (temp_point[2]+m_radius)/(2*m_radius);
				hit = true;
			}
		}
	}
    if(hit){
        intersection->m_dist2 = u_max;
        intersection->m_normal2 = n_max;
    }
	return hit;
}

//a=xD2+yD2, b=2xExD+2yEyD, and c=xE^2+yE^2-1
bool Cylinder::RayIntersect(Ray ray, Intersection *intersection)
{
	Vector3D ray_dir = ray.get_direction();
	Point3D ray_pos = ray.get_position();
	double A = pow(ray_dir[0],2) + pow(ray_dir[2],2);
	double B = 2 * (ray_dir[0]*ray_pos[0] + ray_dir[2]*ray_pos[2]);
	double C = pow(ray_pos[0],2) + pow(ray_pos[2],2) - pow(m_radius,2);
	double roots[2];
	int num_root = quadraticRoots(A,B,C,roots);
	//if(num_root == 0) return false;
	bool hit = false;
    double u_max = INT_MIN;
    Vector3D n_max;
	for(int i=0; i<num_root; i++){
		double u = roots[i];
        if(u > 1e-6 && u_max < u){
            Point3D temp_point = ray.get_position() + u*(ray.get_direction());
            if(temp_point[1] >=0 && temp_point[1] <= m_height){
                u_max = u;
                n_max = temp_point - Point3D(0,temp_point[1],0);
                n_max.normalize(); 
            }
        }
		if(u > 1e-6 && u < intersection->m_dist){
			Point3D temp_point = ray.get_position() + u*(ray.get_direction());
			if(temp_point[1] >= 0 && temp_point[1] <= m_height){
				intersection->m_dist = u;
				intersection->m_intersect = temp_point;
				intersection->m_normal = temp_point - Point3D(0,temp_point[1], 0);
				intersection->m_normal.normalize();
				Vector3D normal = intersection->m_normal;
				double phi = atan2(intersection->m_normal[2], intersection->m_normal[0]);
				if(phi < 0)
					phi = -phi;
				else
					phi = M_PI - phi;
				intersection->u = phi/(M_PI);
				intersection->v = 1.0-temp_point[1]/m_height;
				hit = true;
			}
		}
	}

	if(fabsf(ray_dir[1]) > 1e-6){
		double t = (m_height - ray_pos[1])/ ray_dir[1];
        if(t > 1e-6 && u_max < t){
            Point3D temp_point = ray.get_position() + t*(ray.get_direction());
			if(pow(temp_point[0],2) + pow(temp_point[2],2) <= pow(m_radius,2)){
				u_max = t;
				n_max = Vector3D(0,1,0);
			}
        }
		if(t > 1e-6 && t < intersection->m_dist){
			Point3D temp_point = ray.get_position() + t*(ray.get_direction());
			if(pow(temp_point[0],2) + pow(temp_point[2],2) <= pow(m_radius,2)){
				intersection->m_dist = t;
				intersection->m_intersect = temp_point;
				intersection->m_normal = Vector3D(0,1,0);
				intersection->u = (temp_point[0]+m_radius)/(2*m_radius);
				intersection->v = (temp_point[2]+m_radius)/(2*m_radius);
				hit = true;
			}
		}
		
		t = (0 - ray_pos[1])/ ray_dir[1];
        if(t > -1e-6 && u_max < t){
            Point3D temp_point = ray.get_position() + t*(ray.get_direction());
			if(pow(temp_point[0],2) + pow(temp_point[2],2) <= pow(m_radius,2)){
				u_max = t;
				n_max = Vector3D(0,-1,0);
			}
        }
		if(t > 1e-6 && t < intersection->m_dist){
			Point3D temp_point = ray.get_position() + t*(ray.get_direction());
			if(pow(temp_point[0],2) + pow(temp_point[2],2) <= pow(m_radius,2)){
				intersection->m_dist = t;
				intersection->m_intersect = temp_point;
				intersection->m_normal = Vector3D(0,-1,0);
				intersection->u = (temp_point[0]+m_radius)/(2*m_radius);
				intersection->v = (temp_point[2]+m_radius)/(2*m_radius);
				hit = true;
			}
		}
	}
    if(hit){
        intersection->m_dist2 = u_max;
        intersection->m_normal2 = n_max;
    }
	return hit;
}
/*
 * RAY_LINE_FUNCTION: P[x,y,z] = P1[x1,y1,z1] + u(P2[x2,y2,z2] - P1[x1,y1,z1])
 * SPHERE_FUNCTION: (x-x3)^2 + (y-y3)^2 + (z-z3)^2 = r^2
 * INTERSECTION_FUNCTION: Au^2 + Bu + C = 0
 * -> A=(x2-x1)^2+(y2-y1)^2+(z2-z1)^2 = ray_direction(DOT)ray_direction
 * -> B=2[(x2-x1))(x1-x3)+(y2-y1)(y1-y3)+(z2-z1)(z1-z3)]
 *     =2(ray_diection)(DOT)(pointTOcenter)
 * -> C=x3^2+y3^2+z3^2+x1^2+y1^2+z1^2-2[x3x1+y3y1+z3z1] - radius^2
 *     = pointTOcenter(DOT)pointTOcenter - radius^2
 */
bool NonhierSphere::RayIntersect(Ray ray, Intersection *intersection)
{
	Vector3D pointTOcenter(ray.get_position() - m_pos);
	double A = (ray.get_direction()).dot(ray.get_direction());
	double B = 2*(ray.get_direction()).dot(pointTOcenter);
	double C = pointTOcenter.dot(pointTOcenter) - pow(m_radius,2);
	
	bool hit = false;	
	double roots[2];
	int num_root = quadraticRoots(A,B,C,roots);
	if(num_root == 0) return false;
	for(int i=0; i<num_root; i++){
		double u = roots[i];
		if(u > 1e-6 && u < intersection->m_dist){
			Point3D temp_point = ray.get_position() + u*(ray.get_direction());
			intersection->m_dist = u;
			intersection->m_intersect = temp_point;
			intersection->m_normal = temp_point - m_pos;
			intersection->m_normal.normalize();
			double theta = acos(-intersection->m_normal[1]);
			double phi = atan2(intersection->m_normal[2], intersection->m_normal[0]);
			if(phi < 0)
				phi = -phi;
			else
				phi = M_PI - phi;
			intersection->u = phi/(M_PI);
			intersection->v = (M_PI-theta)/M_PI;
			hit = true;
		}
	}
	if(hit){
		intersection->m_dist2 = INT_MIN;
		for(int i=0; i<num_root; i++){
			double u = roots[i];
			if(u > 1e-6 && u > intersection->m_dist2 && u!=intersection->m_dist){
				intersection->m_dist2 = u;
				Point3D temp_point = ray.get_position() + u*(ray.get_direction());
				intersection->m_normal2 = temp_point - m_pos;
				intersection->m_normal2.normalize();
			}
		}
	}
	return hit;
}

bool NonhierBox::RayIntersect(Ray ray, Intersection *intersection)
{
	double t_near = -std::numeric_limits<double>::infinity();
	double t_far = std::numeric_limits<double>::infinity();
	double t1, t2;
	Vector3D dir = ray.get_direction();
	Point3D pos = ray.get_position();
	Vector3D normal, normal_far;
	int p;
	for(int plane = 0; plane < 3; plane++){
		if(dir[plane] == 0.0 && (pos[plane] < near_vectex[plane] || pos[plane] > far_vectex[plane]))
			return false;
		else{
			t1 = ((double)near_vectex[plane] - (double)pos[plane])/(double)dir[plane];
			t2 = ((double)far_vectex[plane] - (double)pos[plane])/(double)dir[plane];
			if(t1>t2){
				double temp = t1;
				t1 = t2;
				t2 = temp;
			}
            if(t1>t_near){
                t_near = t1;
				p = plane;
                normal = Vector3D(0.0,0.0,0.0);
                normal[plane] = -dir[plane];
            }
            if(t2<t_far){
                t_far = t2;
                normal_far = Vector3D(0.0,0.0,0.0);
                normal_far[plane] = dir[plane];
            }
			if(t_near>t_far) return false;	// box is missed
			if(t_far< 1e-6) return false; 	// box is behind
		}
	}
	if(t_near > 1e-6 && t_near < intersection->m_dist){	
		intersection->m_dist = t_near;
		Point3D hitPoint = pos + t_near*dir;
		intersection->m_intersect = hitPoint;
		intersection->m_normal = normal;
		intersection->m_normal.normalize();
		intersection->m_dist2 = t_far;
		intersection->m_normal2 = normal_far;
		intersection->m_normal2.normalize();
		if(p==0){
			intersection->u = (hitPoint[2] - near_vectex[2])/(far_vectex[2]-near_vectex[2]);
			intersection->v = 1.0 - (hitPoint[1] - near_vectex[1])/(far_vectex[1]-near_vectex[1]);
		}
		else if(p==1){
			intersection->v = (hitPoint[2] - near_vectex[2])/(far_vectex[2]-near_vectex[2]);
			intersection->u = (hitPoint[0] - near_vectex[0])/(far_vectex[0]-near_vectex[0]);
		}
		else if(p==2){
			intersection->v = 1.0 - (hitPoint[1] - near_vectex[1])/(far_vectex[1]-near_vectex[1]);
			intersection->u = (hitPoint[0] - near_vectex[0])/(far_vectex[0]-near_vectex[0]);
		}
		return true;
	}
	else if(t_near <= 1e-6 && t_far < intersection->m_dist){
		intersection->m_dist = t_far;
		intersection->m_intersect = pos+t_far*dir;
		intersection->m_normal = normal_far;
		intersection->m_normal.normalize();
		return true;
	}
	
	return false;
}

bool Sphere::RayIntersect(Ray ray, Intersection *intersection)
{
	return m_sphere->RayIntersect(ray, intersection);
}

bool Cube::RayIntersect(Ray ray, Intersection *intersection)
{
	return m_box->RayIntersect(ray,intersection);
}
